#include "Keyboard.h"
#include "MotorController.h"

#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <chrono>
#include <iostream>


using namespace jetcar;

// Non-blocking single char read
static int readCharNonBlocking() {
    struct termios oldt, newt;
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    int flags = fcntl(STDIN_FILENO, F_GETFL, 0);
    fcntl(STDIN_FILENO, F_SETFL, flags | O_NONBLOCK);

    int c = getchar();

    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    fcntl(STDIN_FILENO, F_SETFL, flags);
    if (c == EOF) return -1;
    return c;
}

void keyboardLoop(std::atomic<int> &speed, std::atomic<int> &direction, const Config &cfg, jetcar::MotorController &motor) {
    using namespace std::chrono;
    std::cout << "Keyboard control: WASD/arrow keys. Hold to move, release to stop. +/- adjust speed. q to quit.\n";
    
    milliseconds debounce_timeout(cfg.keyboard_repeat_timeout_ms); // 防抖阈值，从配置读取
    auto last_press = steady_clock::now() - debounce_timeout * 2;
    int current_dir = 0;
    int last_valid_dir = 0;

    bool quit = false;
    while (!quit) {
        int c = readCharNonBlocking();
        if (c != -1) {
            last_press = steady_clock::now(); // 记录按键时间

            if (c == 'q') { quit = true; break; }
            if (c == '+') { speed = std::min(cfg.max_speed, speed.load() + 100); }
            if (c == '-') { speed = std::max(cfg.min_speed, speed.load() - 100); }

            if (c == 27) { // 处理方向键转义序列
                int c1 = readCharNonBlocking();
                int c2 = readCharNonBlocking();
                if (c1 == 91) {
                    if (c2 == 65)      current_dir = static_cast<int>(MotorController::Direction::FORWARD);  // 上
                    else if (c2 == 66) current_dir = static_cast<int>(MotorController::Direction::BACKWARD); // 下
                    else if (c2 == 67) current_dir = static_cast<int>(MotorController::Direction::RIGHT_BACK); // 右
                    else if (c2 == 68) current_dir = static_cast<int>(MotorController::Direction::LEFT_BACK);  // 左
                    else throw "keyboardLoop: invalid arrow key";
                } else throw "keyboardLoop: invalid escape sequence";
            } else { // 处理WASD等按键
                if (c == 'w')      current_dir = static_cast<int>(MotorController::Direction::FORWARD);
                else if (c == 's') current_dir = static_cast<int>(MotorController::Direction::BACKWARD);
                else if (c == 'a') current_dir = static_cast<int>(MotorController::Direction::LEFT_BACK);
                else if (c == 'd') current_dir = static_cast<int>(MotorController::Direction::RIGHT_BACK);
                else if (c == 'q') current_dir = static_cast<int>(MotorController::Direction::LEFT_FRONT);
                else if (c == 'e') current_dir = static_cast<int>(MotorController::Direction::RIGHT_FRONT);
                else if (c == ' ') current_dir = static_cast<int>(MotorController::Direction::STOP);
                else throw "keyboardLoop: invalid key";
            }

            // 仅当方向变化时，才更新并执行电机控制（核心防抖逻辑）
            if (current_dir != last_valid_dir) {
                last_valid_dir = current_dir;
                direction = current_dir;
                motor.setTarget(current_dir, speed.load());
            }
        } else {
            // 无新按键时，检查防抖超时以释放方向
            if (last_valid_dir != 0) {
                auto now = steady_clock::now();
                if (now - last_press > debounce_timeout) {
                    last_valid_dir = 0;
                    direction = 0;
                    motor.stop();
                }
            }
            std::this_thread::sleep_for(milliseconds(10));
        }
    }
    std::cout << "Keyboard loop exiting\n";
}
